﻿/*
* Box2D.XNA port of Box2D:
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
*
* Original source Box2D:
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com 
* 
* This software is provided 'as-is', without any express or implied 
* warranty.  In no event will the authors be held liable for any damages 
* arising from the use of this software. 
* Permission is granted to anyone to use this software for any purpose, 
* including commercial applications, and to alter it and redistribute it 
* freely, subject to the following restrictions: 
* 1. The origin of this software must not be misrepresented; you must not 
* claim that you wrote the original software. If you use this software 
* in a product, an acknowledgment in the product documentation would be 
* appreciated but is not required. 
* 2. Altered source versions must be plainly marked as such, and must not be 
* misrepresented as being the original software. 
* 3. This notice may not be removed or altered from any source distribution. 
*/

using System;
using System.Diagnostics;
using Microsoft.Xna.Framework;

namespace Box2D.XNA
{
    /// Input parameters for CalculateTimeOfImpact
    public struct TOIInput
    {
        public DistanceProxy proxyA;
        public DistanceProxy proxyB;
        public Sweep sweepA;
        public Sweep sweepB;
        public float tMax; // defines sweep interval [0, tMax]
    };

    public enum TOIOutputState
    {
        Unknown,
        Failed,
        Overlapped,
        Touching,
        Seperated,
    }

    public struct TOIOutput
    {
        public TOIOutputState State;
        public float t;
    }

    public enum SeparationFunctionType
    {
	    Points,
	    FaceA,
	    FaceB
    };

    public struct SeparationFunction
    {
        public SeparationFunction(ref SimplexCache cache,
            ref DistanceProxy proxyA, ref Sweep sweepA,
            ref DistanceProxy proxyB, ref Sweep sweepB,
            float t1)
	    {
            _localPoint = Vector2.Zero;
            _proxyA = proxyA;
            _proxyB = proxyB;
		    int count = cache.count;
		    Debug.Assert(0 < count && count < 3);

            _sweepA = sweepA;
            _sweepB = sweepB;

            Transform xfA, xfB;
            _sweepA.GetTransform(out xfA, t1);
            _sweepB.GetTransform(out xfB, t1);

		    if (count == 1)
		    {
                _type = SeparationFunctionType.Points;
                Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]);
                Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
			    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
			    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
			    _axis = pointB - pointA;
			    _axis.Normalize();
                return;
		    }
		    else if (cache.indexA[0] == cache.indexA[1])
		    {
			    // Two points on B and one on A.
                _type = SeparationFunctionType.FaceB;
                Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

			    _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
			    _axis.Normalize();
                Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);

                _localPoint = 0.5f * (localPointB1 + localPointB2);
			    Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);

                Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
                Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);

			    float s = Vector2.Dot(pointA - pointB, normal);
			    if (s < 0.0f)
			    {
				    _axis = -_axis;
                    s = -s;
			    }
                return;
		    }
		    else
		    {
                // Two points on A and one or two points on B.
			    _type = SeparationFunctionType.FaceA;
			    Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]);
			    Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);
    			
			    _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
			    _axis.Normalize();
			    Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);

			    _localPoint = 0.5f * (localPointA1 + localPointA2);
			    Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);

			    Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
			    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);

			    float s = Vector2.Dot(pointB - pointA, normal);
			    if (s < 0.0f)
			    {
				    _axis = -_axis;
				    s = -s;
			    }
			    return;
		    }
	    }

	    public float FindMinSeparation(out int indexA, out int indexB, float t)
	    {
		    Transform xfA, xfB;
		    _sweepA.GetTransform(out xfA, t);
		    _sweepB.GetTransform(out xfB, t);

		    switch (_type)
		    {
                case SeparationFunctionType.Points:
			    {
				    Vector2 axisA = MathUtils.MultiplyT(ref xfA.R,  _axis);
				    Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);

				    indexA = _proxyA.GetSupport(axisA);
				    indexB = _proxyB.GetSupport(axisB);

				    Vector2 localPointA = _proxyA.GetVertex(indexA);
				    Vector2 localPointB = _proxyB.GetVertex(indexB);
    				
				    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
				    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);

				    float separation = Vector2.Dot(pointB - pointA, _axis);
				    return separation;
			    }

                case SeparationFunctionType.FaceA:
			    {
				    Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
				    Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);

				    Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);
    				
				    indexA = -1;
				    indexB = _proxyB.GetSupport(axisB);

				    Vector2 localPointB = _proxyB.GetVertex(indexB);
				    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);

				    float separation = Vector2.Dot(pointB - pointA, normal);
				    return separation;
			    }

		        case SeparationFunctionType.FaceB:
			    {
				    Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
				    Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);

				    Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);

				    indexB = -1;
				    indexA = _proxyA.GetSupport(axisA);

				    Vector2 localPointA = _proxyA.GetVertex(indexA);
				    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);

				    float separation = Vector2.Dot(pointA - pointB, normal);
				    return separation;
			    }

		        default:
			        Debug.Assert(false);
			        indexA = -1;
			        indexB = -1;
			        return 0.0f;
		    }
	    }
        
        public float Evaluate(int indexA, int indexB, float t)
	    {
            Transform xfA, xfB;
            _sweepA.GetTransform(out xfA, t);
            _sweepB.GetTransform(out xfB, t);

		    switch (_type)
		    {
                case SeparationFunctionType.Points:
			    {
                    Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
                    Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);

                    Vector2 localPointA = _proxyA.GetVertex(indexA);
                    Vector2 localPointB = _proxyB.GetVertex(indexB);
                    
                    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
                    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
                    float separation = Vector2.Dot(pointB - pointA, _axis);

				    return separation;
			    }

                case SeparationFunctionType.FaceA:
			    {
                    Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
                    Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);

                    Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);

                    Vector2 localPointB = _proxyB.GetVertex(indexB);
                    Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);

				    float separation = Vector2.Dot(pointB - pointA, normal);
				    return separation;
			    }

                case SeparationFunctionType.FaceB:
			    {
                    Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
                    Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);

                    Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);

                    Vector2 localPointA = _proxyA.GetVertex(indexA);
                    Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);

				    float separation = Vector2.Dot(pointA - pointB, normal);
				    return separation;
			    }

		    default:
			    Debug.Assert(false);
			    return 0.0f;
		    }
	    }

        DistanceProxy _proxyA;
        DistanceProxy _proxyB;
        Sweep _sweepA, _sweepB; 
        SeparationFunctionType _type;
	    Vector2 _localPoint;
	    Vector2 _axis;
    };



    public static class TimeOfImpact
    {
        // CCD via the local separating axis method. This seeks progression
        // by computing the largest time at which separation is maintained.
        /// Compute the upper bound on time before two shapes penetrate. Time is represented as
        /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
        /// non-tunneling collision. If you change the time interval, you should call this function
        /// again.
        /// Note: use b2Distance to compute the contact point and normal at the time of impact.
        public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input)
        {
	        ++b2_toiCalls;

            output = new TOIOutput();
            output.State = TOIOutputState.Unknown;
            output.t = input.tMax;

	        Sweep sweepA = input.sweepA;
	        Sweep sweepB = input.sweepB;

            // Large rotations can make the root finder fail, so we normalize the
            // sweep angles.
            sweepA.Normalize();
            sweepB.Normalize();

            float tMax = input.tMax;

            float totalRadius = input.proxyA._radius + input.proxyB._radius;
            float target = Math.Max(Settings.b2_linearSlop, totalRadius - 3.0f * Settings.b2_linearSlop);
            float tolerance = 0.25f * Settings.b2_linearSlop;
            Debug.Assert(target > tolerance);

            float t1 = 0.0f;
	        int k_maxIterations = 20;
	        int iter = 0;
	        
	        // Prepare input for distance query.
            SimplexCache cache;
	        DistanceInput distanceInput;
            distanceInput.proxyA = input.proxyA;
            distanceInput.proxyB = input.proxyB;
	        distanceInput.useRadii = false;

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
	        for(;;)
	        {
		        Transform xfA, xfB;
		        sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
		        distanceInput.transformA = xfA;
		        distanceInput.transformB = xfB;
		        DistanceOutput distanceOutput;
		        Distance.ComputeDistance(out distanceOutput, out cache, ref distanceInput);

	            // If the shapes are overlapped, we give up on continuous collision.
	            if (distanceOutput.distance <= 0.0f)
	            {
		            // Failure!
                    output.State = TOIOutputState.Overlapped;
		            output.t = 0.0f;
		            break;
	            }

                if (distanceOutput.distance < target + tolerance)
		        {
			        // Victory!
			        output.State = TOIOutputState.Touching;
			        output.t = t1;
			        break;
		        }

                SeparationFunction fcn = new SeparationFunction(ref cache, ref input.proxyA, ref sweepA, ref input.proxyB, ref sweepB, t1);

                // Compute the TOI on the separating axis. We do this by successively
		        // resolving the deepest point. This loop is bounded by the number of vertices.
		        bool done = false;
		        float t2 = tMax;
                int pushBackIter = 0;
		        for (;;)
		        {
			        // Find the deepest point at t2. Store the witness point indices.
			        int indexA, indexB;
			        float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);

			        // Is the final configuration separated?
			        if (s2 > target + tolerance)
			        {
				        // Victory!
				        output.State = TOIOutputState.Seperated;
				        output.t = tMax;
				        done = true;
				        break;
			        }

			        // Has the separation reached tolerance?
			        if (s2 > target - tolerance)
			        {
				        // Advance the sweeps
				        t1 = t2;
				        break;
			        }

			        // Compute the initial separation of the witness points.
			        float s1 = fcn.Evaluate(indexA, indexB, t1);

			        // Check for initial overlap. This might happen if the root finder
			        // runs out of iterations.
			        if (s1 < target - tolerance)
			        {
				        output.State = TOIOutputState.Failed;
				        output.t = t1;
				        done = true;
				        break;
			        }

			        // Check for touching
			        if (s1 <= target + tolerance)
			        {
				        // Victory! t1 should hold the TOI (could be 0.0).
                        output.State = TOIOutputState.Touching;
				        output.t = t1;
				        done = true;
				        break;
			        }

			        // Compute 1D root of: f(x) - target = 0
			        int rootIterCount = 0;
			        float a1 = t1, a2 = t2;
			        for (;;)
			        {
				        // Use a mix of the secant rule and bisection.
				        float t;
				        if ((rootIterCount & 1) != 0)
				        {
					        // Secant rule to improve convergence.
					        t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
				        }
				        else
				        {
					        // Bisection to guarantee progress.
					        t = 0.5f * (a1 + a2);
				        }

				        float s = fcn.Evaluate(indexA, indexB, t);

				        if (Math.Abs(s - target) < tolerance)
				        {
					        // t2 holds a tentative value for t1
					        t2 = t;
					        break;
				        }

				        // Ensure we continue to bracket the root.
				        if (s > target)
				        {
					        a1 = t;
					        s1 = s;
				        }
				        else
				        {
					        a2 = t;
					        s2 = s;
				        }

				        ++rootIterCount;
				        ++b2_toiRootIters;

				        if (rootIterCount == 50)
				        {
					        break;
				        }
			        }

			        b2_toiMaxRootIters = Math.Max(b2_toiMaxRootIters, rootIterCount);

                    ++pushBackIter;

			        if (pushBackIter == Settings.b2_maxPolygonVertices)
			        {
				        break;
			        }
		        }

		        ++iter;
		        ++b2_toiIters;

		        if (done)
		        {
			        break;
		        }
                
                if (iter == k_maxIterations)
		        {
			        // Root finder got stuck. Semi-victory.
			        output.State = TOIOutputState.Failed;
			        output.t = t1;
			        break;
		        }
	        }

	        b2_toiMaxIters = Math.Max(b2_toiMaxIters, iter);
        }

        public static int b2_toiCalls, b2_toiIters, b2_toiMaxIters;
        public static int b2_toiRootIters, b2_toiMaxRootIters;
        public static int b2_toiMaxOptIters;
    }
}
